/// @file
/// Similarity group Sim(3) - scaling, rotation and translation in 3d.

#ifndef SOPHUS_SIM3_HPP
#define SOPHUS_SIM3_HPP

#include "rxso3.hpp"
#include "sim_details.hpp"

namespace Sophus
{
  template <class Scalar_, int Options = 0>
  class Sim3;
  using Sim3d = Sim3<double>;
  using Sim3f = Sim3<float>;
} // namespace Sophus

namespace Eigen
{
  namespace internal
  {
    template <class Scalar_, int Options>
    struct traits<Sophus::Sim3<Scalar_, Options>>
    {
      using Scalar = Scalar_;
      using TranslationType = Sophus::Vector3<Scalar, Options>;
      using RxSO3Type = Sophus::RxSO3<Scalar, Options>;
    };

    template <class Scalar_, int Options>
    struct traits<Map<Sophus::Sim3<Scalar_>, Options>>
        : traits<Sophus::Sim3<Scalar_, Options>>
    {
      using Scalar = Scalar_;
      using TranslationType = Map<Sophus::Vector3<Scalar>, Options>;
      using RxSO3Type = Map<Sophus::RxSO3<Scalar>, Options>;
    };

    template <class Scalar_, int Options>
    struct traits<Map<Sophus::Sim3<Scalar_> const, Options>>
        : traits<Sophus::Sim3<Scalar_, Options> const>
    {
      using Scalar = Scalar_;
      using TranslationType = Map<Sophus::Vector3<Scalar> const, Options>;
      using RxSO3Type = Map<Sophus::RxSO3<Scalar> const, Options>;
    };
  } // namespace internal
} // namespace Eigen

namespace Sophus
{
  /// Sim3 base type - implements Sim3 class but is storage agnostic.
  ///
  /// Sim(3) is the group of rotations  and translation and scaling in 3d. It is
  /// the semi-direct product of R+xSO(3) and the 3d Euclidean vector space.  The
  /// class is represented using a composition of RxSO3  for scaling plus
  /// rotation and a 3-vector for translation.
  ///
  /// Sim(3) is neither compact, nor a commutative group.
  ///
  /// See RxSO3 for more details of the scaling + rotation representation in 3d.
  ///
  template <class Derived>
  class Sim3Base
  {
  public:
    using Scalar = typename Eigen::internal::traits<Derived>::Scalar;
    using TranslationType =
        typename Eigen::internal::traits<Derived>::TranslationType;
    using RxSO3Type = typename Eigen::internal::traits<Derived>::RxSO3Type;
    using QuaternionType = typename RxSO3Type::QuaternionType;

    /// Degrees of freedom of manifold, number of dimensions in tangent space
    /// (three for translation, three for rotation and one for scaling).
    static int constexpr DoF = 7;
    /// Number of internal parameters used (4-tuple for quaternion, three for
    /// translation).
    static int constexpr num_parameters = 7;
    /// Group transformations are 4x4 matrices.
    static int constexpr N = 4;
    using Transformation = Matrix<Scalar, N, N>;
    using Point = Vector3<Scalar>;
    using HomogeneousPoint = Vector4<Scalar>;
    using Line = ParametrizedLine3<Scalar>;
    using Tangent = Vector<Scalar, DoF>;
    using Adjoint = Matrix<Scalar, DoF, DoF>;

    /// For binary operations the return type is determined with the
    /// ScalarBinaryOpTraits feature of Eigen. This allows mixing concrete and Map
    /// types, as well as other compatible scalar types such as Ceres::Jet and
    /// double scalars with Sim3 operations.
    template <typename OtherDerived>
    using ReturnScalar = typename Eigen::ScalarBinaryOpTraits<
        Scalar, typename OtherDerived::Scalar>::ReturnType;

    template <typename OtherDerived>
    using Sim3Product = Sim3<ReturnScalar<OtherDerived>>;

    template <typename PointDerived>
    using PointProduct = Vector3<ReturnScalar<PointDerived>>;

    template <typename HPointDerived>
    using HomogeneousPointProduct = Vector4<ReturnScalar<HPointDerived>>;

    /// Adjoint transformation
    ///
    /// This function return the adjoint transformation ``Ad`` of the group
    /// element ``A`` such that for all ``x`` it holds that
    /// ``hat(Ad_A * x) = A * hat(x) A^{-1}``. See hat-operator below.
    ///
    SOPHUS_FUNC Adjoint Adj() const
    {
      Matrix3<Scalar> const R = rxso3().rotationMatrix();
      Adjoint res;
      res.setZero();
      res.template block<3, 3>(0, 0) = rxso3().matrix();
      res.template block<3, 3>(0, 3) = SO3<Scalar>::hat(translation()) * R;
      res.template block<3, 1>(0, 6) = -translation();
      res.template block<3, 3>(3, 3) = R;

      res(6, 6) = Scalar(1);

      return res;
    }

    /// Returns copy of instance casted to NewScalarType.
    ///
    template <class NewScalarType>
    SOPHUS_FUNC Sim3<NewScalarType> cast() const
    {
      return Sim3<NewScalarType>(rxso3().template cast<NewScalarType>(),
                                 translation().template cast<NewScalarType>());
    }

    /// Returns group inverse.
    ///
    SOPHUS_FUNC Sim3<Scalar> inverse() const
    {
      RxSO3<Scalar> invR = rxso3().inverse();
      return Sim3<Scalar>(invR, invR * (translation() * Scalar(-1)));
    }

    /// Logarithmic map
    ///
    /// Computes the logarithm, the inverse of the group exponential which maps
    /// element of the group (rigid body transformations) to elements of the
    /// tangent space (twist).
    ///
    /// To be specific, this function computes ``vee(logmat(.))`` with
    /// ``logmat(.)`` being the matrix logarithm and ``vee(.)`` the vee-operator
    /// of Sim(3).
    ///
    SOPHUS_FUNC Tangent log() const
    {
      // The derivation of the closed-form Sim(3) logarithm for is done
      // analogously to the closed-form solution of the SE(3) logarithm, see
      // J. Gallier, D. Xu, "Computing exponentials of skew symmetric matrices
      // and logarithms of orthogonal matrices", IJRA 2002.
      // https:///pdfs.semanticscholar.org/cfe3/e4b39de63c8cabd89bf3feff7f5449fc981d.pdf
      // (Sec. 6., pp. 8)
      Tangent res;
      auto omega_sigma_and_theta = rxso3().logAndTheta();
      Vector3<Scalar> const omega =
          omega_sigma_and_theta.tangent.template head<3>();
      Scalar sigma = omega_sigma_and_theta.tangent[3];
      Matrix3<Scalar> const Omega = SO3<Scalar>::hat(omega);
      Matrix3<Scalar> const W_inv = details::calcWInv<Scalar, 3>(
          Omega, omega_sigma_and_theta.theta, sigma, scale());

      res.segment(0, 3) = W_inv * translation();
      res.segment(3, 3) = omega;

      res[6] = sigma;

      return res;
    }

    /// Returns 4x4 matrix representation of the instance.
    ///
    /// It has the following form:
    ///
    ///     | s*R t |
    ///     |  o  1 |
    ///
    /// where ``R`` is a 3x3 rotation matrix, ``s`` a scale factor, ``t`` a
    /// translation 3-vector and ``o`` a 3-column vector of zeros.
    ///
    SOPHUS_FUNC Transformation matrix() const
    {
      Transformation homogenious_matrix;
      homogenious_matrix.template topLeftCorner<3, 4>() = matrix3x4();
      homogenious_matrix.row(3) =
          Matrix<Scalar, 4, 1>(Scalar(0), Scalar(0), Scalar(0), Scalar(1));

      return homogenious_matrix;
    }

    /// Returns the significant first three rows of the matrix above.
    ///
    SOPHUS_FUNC Matrix<Scalar, 3, 4> matrix3x4() const
    {
      Matrix<Scalar, 3, 4> matrix;
      matrix.template topLeftCorner<3, 3>() = rxso3().matrix();
      matrix.col(3) = translation();

      return matrix;
    }

    /// Assignment operator.
    ///
    SOPHUS_FUNC Sim3Base &operator=(Sim3Base const &other) = default;

    /// Assignment-like operator from OtherDerived.
    ///
    template <class OtherDerived>
    SOPHUS_FUNC Sim3Base<Derived> &operator=(
        Sim3Base<OtherDerived> const &other)
    {
      rxso3() = other.rxso3();
      translation() = other.translation();

      return *this;
    }

    /// Group multiplication, which is rotation plus scaling concatenation.
    ///
    /// Note: That scaling is calculated with saturation. See RxSO3 for
    /// details.
    ///
    template <typename OtherDerived>
    SOPHUS_FUNC Sim3Product<OtherDerived> operator*(
        Sim3Base<OtherDerived> const &other) const
    {
      return Sim3Product<OtherDerived>(
          rxso3() * other.rxso3(), translation() + rxso3() * other.translation());
    }

    /// Group action on 3-points.
    ///
    /// This function rotates, scales and translates a three dimensional point
    /// ``p`` by the Sim(3) element ``(bar_sR_foo, t_bar)`` (= similarity
    /// transformation):
    ///
    ///   ``p_bar = bar_sR_foo * p_foo + t_bar``.
    ///
    template <typename PointDerived,
              typename = typename std::enable_if<
                  IsFixedSizeVector<PointDerived, 3>::value>::type>
    SOPHUS_FUNC PointProduct<PointDerived> operator*(
        Eigen::MatrixBase<PointDerived> const &p) const
    {
      return rxso3() * p + translation();
    }

    /// Group action on homogeneous 3-points. See above for more details.
    ///
    template <typename HPointDerived,
              typename = typename std::enable_if<
                  IsFixedSizeVector<HPointDerived, 4>::value>::type>
    SOPHUS_FUNC HomogeneousPointProduct<HPointDerived> operator*(
        Eigen::MatrixBase<HPointDerived> const &p) const
    {
      const PointProduct<HPointDerived> tp =
          rxso3() * p.template head<3>() + p(3) * translation();
      return HomogeneousPointProduct<HPointDerived>(tp(0), tp(1), tp(2), p(3));
    }

    /// Group action on lines.
    ///
    /// This function rotates, scales and translates a parametrized line
    /// ``l(t) = o + t * d`` by the Sim(3) element:
    ///
    /// Origin ``o`` is rotated, scaled and translated
    /// Direction ``d`` is rotated
    ///
    SOPHUS_FUNC Line operator*(Line const &l) const
    {
      Line rotatedLine = rxso3() * l;
      return Line(rotatedLine.origin() + translation(), rotatedLine.direction());
    }

    /// In-place group multiplication. This method is only valid if the return
    /// type of the multiplication is compatible with this SO3's Scalar type.
    ///
    template <typename OtherDerived,
              typename = typename std::enable_if<
                  std::is_same<Scalar, ReturnScalar<OtherDerived>>::value>::type>
    SOPHUS_FUNC Sim3Base<Derived> &operator*=(
        Sim3Base<OtherDerived> const &other)
    {
      *static_cast<Derived *>(this) = *this * other;
      return *this;
    }

    /// Returns internal parameters of Sim(3).
    ///
    /// It returns (q.imag[0], q.imag[1], q.imag[2], q.real, t[0], t[1], t[2]),
    /// with q being the quaternion, t the translation 3-vector.
    ///
    SOPHUS_FUNC Sophus::Vector<Scalar, num_parameters> params() const
    {
      Sophus::Vector<Scalar, num_parameters> p;
      p << rxso3().params(), translation();
      return p;
    }

    /// Setter of non-zero quaternion.
    ///
    /// Precondition: ``quat`` must not be close to zero.
    ///
    SOPHUS_FUNC void setQuaternion(Eigen::Quaternion<Scalar> const &quat)
    {
      rxso3().setQuaternion(quat);
    }

    /// Accessor of quaternion.
    ///
    SOPHUS_FUNC QuaternionType const &quaternion() const
    {
      return rxso3().quaternion();
    }

    /// Returns Rotation matrix
    ///
    SOPHUS_FUNC Matrix3<Scalar> rotationMatrix() const
    {
      return rxso3().rotationMatrix();
    }

    /// Mutator of SO3 group.
    ///
    SOPHUS_FUNC RxSO3Type &rxso3()
    {
      return static_cast<Derived *>(this)->rxso3();
    }

    /// Accessor of SO3 group.
    ///
    SOPHUS_FUNC RxSO3Type const &rxso3() const
    {
      return static_cast<Derived const *>(this)->rxso3();
    }

    /// Returns scale.
    ///
    SOPHUS_FUNC Scalar scale() const { return rxso3().scale(); }

    /// Setter of quaternion using rotation matrix ``R``, leaves scale as is.
    ///
    SOPHUS_FUNC void setRotationMatrix(Matrix3<Scalar> &R)
    {
      rxso3().setRotationMatrix(R);
    }

    /// Sets scale and leaves rotation as is.
    ///
    /// Note: This function as a significant computational cost, since it has to
    /// call the square root twice.
    ///
    SOPHUS_FUNC void setScale(Scalar const &scale) { rxso3().setScale(scale); }

    /// Setter of quaternion using scaled rotation matrix ``sR``.
    ///
    /// Precondition: The 3x3 matrix must be "scaled orthogonal"
    ///               and have a positive determinant.
    ///
    SOPHUS_FUNC void setScaledRotationMatrix(Matrix3<Scalar> const &sR)
    {
      rxso3().setScaledRotationMatrix(sR);
    }

    /// Mutator of translation vector
    ///
    SOPHUS_FUNC TranslationType &translation()
    {
      return static_cast<Derived *>(this)->translation();
    }

    /// Accessor of translation vector
    ///
    SOPHUS_FUNC TranslationType const &translation() const
    {
      return static_cast<Derived const *>(this)->translation();
    }
  };

  /// Sim3 using default storage; derived from Sim3Base.
  template <class Scalar_, int Options>
  class Sim3 : public Sim3Base<Sim3<Scalar_, Options>>
  {
  public:
    using Base = Sim3Base<Sim3<Scalar_, Options>>;
    using Scalar = Scalar_;
    using Transformation = typename Base::Transformation;
    using Point = typename Base::Point;
    using HomogeneousPoint = typename Base::HomogeneousPoint;
    using Tangent = typename Base::Tangent;
    using Adjoint = typename Base::Adjoint;
    using RxSo3Member = RxSO3<Scalar, Options>;
    using TranslationMember = Vector3<Scalar, Options>;

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    /// Default constructor initializes similarity transform to the identity.
    ///
    SOPHUS_FUNC Sim3();

    /// Copy constructor
    ///
    SOPHUS_FUNC Sim3(Sim3 const &other) = default;

    /// Copy-like constructor from OtherDerived.
    ///
    template <class OtherDerived>
    SOPHUS_FUNC Sim3(Sim3Base<OtherDerived> const &other)
        : rxso3_(other.rxso3()), translation_(other.translation())
    {
      static_assert(std::is_same<typename OtherDerived::Scalar, Scalar>::value,
                    "must be same Scalar type");
    }

    /// Constructor from RxSO3 and translation vector
    ///
    template <class OtherDerived, class D>
    SOPHUS_FUNC Sim3(RxSO3Base<OtherDerived> const &rxso3,
                     Eigen::MatrixBase<D> const &translation)
        : rxso3_(rxso3), translation_(translation)
    {
      static_assert(std::is_same<typename OtherDerived::Scalar, Scalar>::value,
                    "must be same Scalar type");
      static_assert(std::is_same<typename D::Scalar, Scalar>::value,
                    "must be same Scalar type");
    }

    /// Constructor from quaternion and translation vector.
    ///
    /// Precondition: quaternion must not be close to zero.
    ///
    template <class D1, class D2>
    SOPHUS_FUNC Sim3(Eigen::QuaternionBase<D1> const &quaternion,
                     Eigen::MatrixBase<D2> const &translation)
        : rxso3_(quaternion), translation_(translation)
    {
      static_assert(std::is_same<typename D1::Scalar, Scalar>::value,
                    "must be same Scalar type");
      static_assert(std::is_same<typename D2::Scalar, Scalar>::value,
                    "must be same Scalar type");
    }

    /// Constructor from 4x4 matrix
    ///
    /// Precondition: Top-left 3x3 matrix needs to be "scaled-orthogonal" with
    ///               positive determinant. The last row must be ``(0, 0, 0, 1)``.
    ///
    SOPHUS_FUNC explicit Sim3(Matrix<Scalar, 4, 4> const &T)
        : rxso3_(T.template topLeftCorner<3, 3>()),
          translation_(T.template block<3, 1>(0, 3)) {}

    /// This provides unsafe read/write access to internal data. Sim(3) is
    /// represented by an Eigen::Quaternion (four parameters) and a 3-vector. When
    /// using direct write access, the user needs to take care of that the
    /// quaternion is not set close to zero.
    ///
    SOPHUS_FUNC Scalar *data()
    {
      // rxso3_ and translation_ are laid out sequentially with no padding
      return rxso3_.data();
    }

    /// Const version of data() above.
    ///
    SOPHUS_FUNC Scalar const *data() const
    {
      // rxso3_ and translation_ are laid out sequentially with no padding
      return rxso3_.data();
    }

    /// Accessor of RxSO3
    ///
    SOPHUS_FUNC RxSo3Member &rxso3() { return rxso3_; }

    /// Mutator of RxSO3
    ///
    SOPHUS_FUNC RxSo3Member const &rxso3() const { return rxso3_; }

    /// Mutator of translation vector
    ///
    SOPHUS_FUNC TranslationMember &translation() { return translation_; }

    /// Accessor of translation vector
    ///
    SOPHUS_FUNC TranslationMember const &translation() const
    {
      return translation_;
    }

    /// Returns derivative of exp(x).matrix() wrt. ``x_i at x=0``.
    ///
    SOPHUS_FUNC static Transformation Dxi_exp_x_matrix_at_0(int i)
    {
      return generator(i);
    }

    /// Group exponential
    ///
    /// This functions takes in an element of tangent space and returns the
    /// corresponding element of the group Sim(3).
    ///
    /// The first three components of ``a`` represent the translational part
    /// ``upsilon`` in the tangent space of Sim(3), the following three components
    /// of ``a`` represents the rotation vector ``omega`` and the final component
    /// represents the logarithm of the scaling factor ``sigma``.
    /// To be more specific, this function computes ``expmat(hat(a))`` with
    /// ``expmat(.)`` being the matrix exponential and ``hat(.)`` the hat-operator
    /// of Sim(3), see below.
    ///
    SOPHUS_FUNC static Sim3<Scalar> exp(Tangent const &a)
    {
      // For the derivation of the exponential map of Sim(3) see
      // H. Strasdat, "Local Accuracy and Global Consistency for Efficient Visual
      // SLAM", PhD thesis, 2012.
      // http:///hauke.strasdat.net/files/strasdat_thesis_2012.pdf (A.5, pp. 186)
      Vector3<Scalar> const upsilon = a.segment(0, 3);
      Vector3<Scalar> const omega = a.segment(3, 3);
      Scalar const sigma = a[6];
      Scalar theta;
      RxSO3<Scalar> rxso3 =
          RxSO3<Scalar>::expAndTheta(a.template tail<4>(), &theta);
      Matrix3<Scalar> const Omega = SO3<Scalar>::hat(omega);

      Matrix3<Scalar> const W = details::calcW<Scalar, 3>(Omega, theta, sigma);

      return Sim3<Scalar>(rxso3, W * upsilon);
    }

    /// Returns the ith infinitesimal generators of Sim(3).
    ///
    /// The infinitesimal generators of Sim(3) are:
    ///
    /// ```
    ///         |  0  0  0  1 |
    ///   G_0 = |  0  0  0  0 |
    ///         |  0  0  0  0 |
    ///         |  0  0  0  0 |
    ///
    ///         |  0  0  0  0 |
    ///   G_1 = |  0  0  0  1 |
    ///         |  0  0  0  0 |
    ///         |  0  0  0  0 |
    ///
    ///         |  0  0  0  0 |
    ///   G_2 = |  0  0  0  0 |
    ///         |  0  0  0  1 |
    ///         |  0  0  0  0 |
    ///
    ///         |  0  0  0  0 |
    ///   G_3 = |  0  0 -1  0 |
    ///         |  0  1  0  0 |
    ///         |  0  0  0  0 |
    ///
    ///         |  0  0  1  0 |
    ///   G_4 = |  0  0  0  0 |
    ///         | -1  0  0  0 |
    ///         |  0  0  0  0 |
    ///
    ///         |  0 -1  0  0 |
    ///   G_5 = |  1  0  0  0 |
    ///         |  0  0  0  0 |
    ///         |  0  0  0  0 |
    ///
    ///         |  1  0  0  0 |
    ///   G_6 = |  0  1  0  0 |
    ///         |  0  0  1  0 |
    ///         |  0  0  0  0 |
    /// ```
    ///
    /// Precondition: ``i`` must be in [0, 6].
    ///
    SOPHUS_FUNC static Transformation generator(int i)
    {
      SOPHUS_ENSURE(i >= 0 || i <= 6, "i should be in range [0,6].");
      Tangent e;
      e.setZero();
      e[i] = Scalar(1);

      return hat(e);
    }

    /// hat-operator
    ///
    /// It takes in the 7-vector representation and returns the corresponding
    /// matrix representation of Lie algebra element.
    ///
    /// Formally, the hat()-operator of Sim(3) is defined as
    ///
    ///   ``hat(.): R^7 -> R^{4x4},  hat(a) = sum_i a_i * G_i``  (for i=0,...,6)
    ///
    /// with ``G_i`` being the ith infinitesimal generator of Sim(3).
    ///
    /// The corresponding inverse is the vee()-operator, see below.
    ///
    SOPHUS_FUNC static Transformation hat(Tangent const &a)
    {
      Transformation Omega;
      Omega.template topLeftCorner<3, 3>() =
          RxSO3<Scalar>::hat(a.template tail<4>());
      Omega.col(3).template head<3>() = a.template head<3>();
      Omega.row(3).setZero();

      return Omega;
    }

    /// Lie bracket
    ///
    /// It computes the Lie bracket of Sim(3). To be more specific, it computes
    ///
    ///   ``[omega_1, omega_2]_sim3 := vee([hat(omega_1), hat(omega_2)])``
    ///
    /// with ``[A,B] := AB-BA`` being the matrix commutator, ``hat(.)`` the
    /// hat()-operator and ``vee(.)`` the vee()-operator of Sim(3).
    ///
    SOPHUS_FUNC static Tangent lieBracket(Tangent const &a, Tangent const &b)
    {
      Vector3<Scalar> const upsilon1 = a.template head<3>();
      Vector3<Scalar> const upsilon2 = b.template head<3>();
      Vector3<Scalar> const omega1 = a.template segment<3>(3);
      Vector3<Scalar> const omega2 = b.template segment<3>(3);
      Scalar sigma1 = a[6];
      Scalar sigma2 = b[6];

      Tangent res;
      res.template head<3>() = SO3<Scalar>::hat(omega1) * upsilon2 +
                               SO3<Scalar>::hat(upsilon1) * omega2 +
                               sigma1 * upsilon2 - sigma2 * upsilon1;
      res.template segment<3>(3) = omega1.cross(omega2);
      res[6] = Scalar(0);

      return res;
    }

    /// Draw uniform sample from Sim(3) manifold.
    ///
    /// Translations are drawn component-wise from the range [-1, 1].
    /// The scale factor is drawn uniformly in log2-space from [-1, 1],
    /// hence the scale is in [0.5, 2].
    ///
    template <class UniformRandomBitGenerator>
    static Sim3 sampleUniform(UniformRandomBitGenerator &generator)
    {
      std::uniform_real_distribution<Scalar> uniform(Scalar(-1), Scalar(1));
      return Sim3(RxSO3<Scalar>::sampleUniform(generator),
                  Vector3<Scalar>(uniform(generator), uniform(generator),
                                  uniform(generator)));
    }

    /// vee-operator
    ///
    /// It takes the 4x4-matrix representation ``Omega`` and maps it to the
    /// corresponding 7-vector representation of Lie algebra.
    ///
    /// This is the inverse of the hat()-operator, see above.
    ///
    /// Precondition: ``Omega`` must have the following structure:
    ///
    ///                |  g -f  e  a |
    ///                |  f  g -d  b |
    ///                | -e  d  g  c |
    ///                |  0  0  0  0 |
    ///
    SOPHUS_FUNC static Tangent vee(Transformation const &Omega)
    {
      Tangent upsilon_omega_sigma;
      upsilon_omega_sigma.template head<3>() = Omega.col(3).template head<3>();
      upsilon_omega_sigma.template tail<4>() =
          RxSO3<Scalar>::vee(Omega.template topLeftCorner<3, 3>());

      return upsilon_omega_sigma;
    }

  protected:
    RxSo3Member rxso3_;
    TranslationMember translation_;
  };

  template <class Scalar, int Options>
  Sim3<Scalar, Options>::Sim3() : translation_(TranslationMember::Zero())
  {
    static_assert(std::is_standard_layout<Sim3>::value,
                  "Assume standard layout for the use of offsetof check below.");
    static_assert(
        offsetof(Sim3, rxso3_) + sizeof(Scalar) * RxSO3<Scalar>::num_parameters ==
            offsetof(Sim3, translation_),
        "This class assumes packed storage and hence will only work "
        "correctly depending on the compiler (options) - in "
        "particular when using [this->data(), this-data() + "
        "num_parameters] to access the raw data in a contiguous fashion.");
  }
} // namespace Sophus

namespace Eigen
{
  /// Specialization of Eigen::Map for ``Sim3``; derived from Sim3Base.
  ///
  /// Allows us to wrap Sim3 objects around POD array.
  template <class Scalar_, int Options>
  class Map<Sophus::Sim3<Scalar_>, Options>
      : public Sophus::Sim3Base<Map<Sophus::Sim3<Scalar_>, Options>>
  {
  public:
    using Base = Sophus::Sim3Base<Map<Sophus::Sim3<Scalar_>, Options>>;
    using Scalar = Scalar_;
    using Transformation = typename Base::Transformation;
    using Point = typename Base::Point;
    using HomogeneousPoint = typename Base::HomogeneousPoint;
    using Tangent = typename Base::Tangent;
    using Adjoint = typename Base::Adjoint;

    // LCOV_EXCL_START
    SOPHUS_INHERIT_ASSIGNMENT_OPERATORS(Map);
    // LCOV_EXCL_STOP

    using Base::operator*=;
    using Base::operator*;

    SOPHUS_FUNC Map(Scalar *coeffs)
        : rxso3_(coeffs),
          translation_(coeffs + Sophus::RxSO3<Scalar>::num_parameters) {}

    /// Mutator of RxSO3
    ///
    SOPHUS_FUNC Map<Sophus::RxSO3<Scalar>, Options> &rxso3() { return rxso3_; }

    /// Accessor of RxSO3
    ///
    SOPHUS_FUNC Map<Sophus::RxSO3<Scalar>, Options> const &rxso3() const
    {
      return rxso3_;
    }

    /// Mutator of translation vector
    ///
    SOPHUS_FUNC Map<Sophus::Vector3<Scalar>, Options> &translation()
    {
      return translation_;
    }

    /// Accessor of translation vector
    SOPHUS_FUNC Map<Sophus::Vector3<Scalar>, Options> const &translation() const
    {
      return translation_;
    }

  protected:
    Map<Sophus::RxSO3<Scalar>, Options> rxso3_;
    Map<Sophus::Vector3<Scalar>, Options> translation_;
  };

  /// Specialization of Eigen::Map for ``Sim3 const``; derived from Sim3Base.
  ///
  /// Allows us to wrap RxSO3 objects around POD array.
  template <class Scalar_, int Options>
  class Map<Sophus::Sim3<Scalar_> const, Options>
      : public Sophus::Sim3Base<Map<Sophus::Sim3<Scalar_> const, Options>>
  {
    using Base = Sophus::Sim3Base<Map<Sophus::Sim3<Scalar_> const, Options>>;

  public:
    using Scalar = Scalar_;
    using Transformation = typename Base::Transformation;
    using Point = typename Base::Point;
    using HomogeneousPoint = typename Base::HomogeneousPoint;
    using Tangent = typename Base::Tangent;
    using Adjoint = typename Base::Adjoint;

    using Base::operator*=;
    using Base::operator*;

    SOPHUS_FUNC Map(Scalar const *coeffs)
        : rxso3_(coeffs),
          translation_(coeffs + Sophus::RxSO3<Scalar>::num_parameters) {}

    /// Accessor of RxSO3
    ///
    SOPHUS_FUNC Map<Sophus::RxSO3<Scalar> const, Options> const &rxso3() const
    {
      return rxso3_;
    }

    /// Accessor of translation vector
    ///
    SOPHUS_FUNC Map<Sophus::Vector3<Scalar> const, Options> const &translation()
        const
    {
      return translation_;
    }

  protected:
    Map<Sophus::RxSO3<Scalar> const, Options> const rxso3_;
    Map<Sophus::Vector3<Scalar> const, Options> const translation_;
  };
} // namespace Eigen

#endif
